using System;
using Microsoft.SPOT;
using MLRobotic.IA.Utils;

namespace MLRobotic.IA.Goals
{
    public class MoveTo : Goal
    {
        private int _x;
        private int _y;
        private bool _backward;
        private int _retry;

        public int MaxError { get; set; }

        public MoveTo(int x, int y, bool backward = false, bool blind = false)
            : base(blind)
        {
            _x = x;
            _y = y;
            _backward = backward;

            MaxError = 100;
        }
    
        public override void Process()
        {
            if (State == GoalSate.Todo)
            {
                State = GoalSate.Doing;
                Com.MoveTo(_x, _y, _backward);
            }
            else if (State == GoalSate.Doing)
            {
                if (Robot.NotifyFinDeplace)
                {
                    if (TrigoExt.Distance(_x, _y, Robot.X, Robot.Y) < MaxError)
                    {
                        State = GoalSate.Done;
                    }
                    else if (_retry < 1)
                    {
                        _retry++;
                        Com.MoveTo(_x, _y, _backward);
                    }
                    else
                    {
                        Timeout = true;
                        State = GoalSate.Done;
                    }
                }
            }
        }
    }
}
